#include "preheader.hpp"
#include "window.hpp"

#include "hpp/OB2Context.hpp"
#include <mutex>
#include <thread>
using namespace std;
// OB2_IMAGES_SYNC_MODE_WAIT_LATER_COMER
ob2_images_sync_mode_t images_sync_mode = OB2_IMAGES_SYNC_MODE_DEVICE_TIMESTAMP_MATCH;
float alpha = 0.5;
char letter = -1;
ob2_images_align_mode_t align_mode = OB2_IMAGES_ALIGN_MODE_DISABLE;
// OB2_IMAGES_ALIGN_MODE_D2C_HARDWARE  OB2_IMAGES_ALIGN_MODE_D2C_SOFTWARE
std::mutex capture_mutex;
std::shared_ptr<ob2::capture> capture;
bool thread_quit = false;

using namespace std;
#include <opencv2/opencv.hpp>
#include <vector>
#include <fstream>
#include <map>
#include <chrono>
#include <string>
#include <sys/time.h>
struct NowDate
{
    char tmp0[16];       // 年月日
    char tmp1[16];       // 时分秒
    char filename[128];  // 年月日时分秒
    char rgbname[256];   // 年月日时分秒
    char depthname[256]; // 年月日时分秒

    struct timeval tv;
    string fn;
    NowDate()
    {
        update();
        fn = string(filename);
    }
    time_t timep;
    void update()
    {
        time(&timep);
        gettimeofday(&tv, NULL);
        // strftime(tmp0, sizeof(tmp0), "%Y_%m_%d", localtime(&timep));
        // strftime(tmp1, sizeof(tmp1), "%H_%M_%S", localtime(&timep));
        strftime(filename, sizeof(filename), "%Y_%m_%d_%H_%M_%S", localtime(&timep));
        sprintf(rgbname, "/img/%s_%ld_dep.png", filename, tv.tv_usec);
        sprintf(depthname, "/img/%s_%ld_rgb.png", filename, tv.tv_usec);

        // printf("%s %s\n", filename, depthname, rgbname);
    }

    void readfiles(vector<string> &rgbs, string path)
    {
        fstream fs(path + "/file.txt", std::ios::in);

        while (1)
        {
            string n, n2;
            fs >> n >> n2;
            if (!fs.good())
                break;
            rgbs.push_back(n);
            rgbs.push_back(n2);
        }
        // rgbs.pop_back
    }
};

// This example may cause the depth or color camera to not support mirror, resulting in the depth map and color map mirror status inconsistent, resulting in the
// depth map and color map display image is opposite
int main(int argc, char **argv) TRY_EXECUTE
{
    // Used to manage the SDK lifecycle
    auto ctx = std::make_shared<ob2::context>();

    // Open default device
    auto dev = ctx->open_device(OB2_DEFAULT_DEVICE);

    // Create camera configuration
    auto cameras_config = dev->create_cameras_config();

    // Enable color camera (use default stream configuration, or configure a specific format of data stream through the set_camera_stream_profile interface)
    // cameras_config->enable_camera_stream(OB2_CAMERA_COLOR);
    cameras_config->enable_camera_stream(OB2_CAMERA_COLOR, 640, 480, OB2_ANY_FRAME_RATE, OB2_FORMAT_RGB);

    auto ps = dev->get_camera_stream_profile_list(OB2_CAMERA_DEPTH);
    std::cout << ps.size() << std::endl;
    // Configure the data stream format of the depth camera
    cameras_config->set_camera_stream_profile(OB2_CAMERA_DEPTH, ps[2]);

    // Enable depth camera (use default stream configuration, or configure a specific format of data stream through the set_camera_stream_profile interface)
    cameras_config->enable_camera_stream(OB2_CAMERA_DEPTH);

    // Configure the alignment mode to software D2C alignment
    align_mode = OB2_IMAGES_ALIGN_MODE_D2C_SOFTWARE;
    cameras_config->set_images_align_mode(OB2_IMAGES_ALIGN_MODE_D2C_HARDWARE);

    cameras_config->set_images_sync_mode(OB2_IMAGES_SYNC_MODE_WAIT_LATER_COMER);
    dev->update_cameras_config(cameras_config);

    // Start camera
    dev->start_cameras(cameras_config);
    NowDate date;
    system(cv::format("mkdir -p %s/img", date.fn.c_str()).c_str());

    // Create a window for rendering and set the window resolution
    Window app("SyncAlignViewer", 640, 480);

    // Create a thread to get capture from SDK internal to prevent SDK internal buffer overflow and discard data due to subsequent processing time too long
    std::thread waitFrameThread([&]()
                                {
        while(!thread_quit) {
            // Get capture
            std::shared_ptr<ob2::capture> capture_t;
            try {
                capture_t = dev->get_capture(100);
            }
            catch(const std::runtime_error &e) {
                // Maybe timeout
                std::cerr << "Get capture failed! msg=" << e.what() << std::endl;
                continue;
            }

            std::unique_lock<std::mutex> lock(capture_mutex, std::defer_lock);
            if(lock.try_lock()) {
                capture = capture_t;
            }
        } });
    int cnt = 0;

    fstream fs(date.fn + "/file.txt", std::ios::out);

    while (app)
    {
        // Get the key value of the key event
        char key = cv::waitKey(100);

        letter = key;

        if (capture != nullptr)
        {
            std::unique_lock<std::mutex> lock(capture_mutex);
            // Render a group of frame data in the window, here will render color frame and depth frame
            std::shared_ptr<ob2::image> color_image = capture->get_color_image();
            std::shared_ptr<ob2::image> depth_image = capture->get_depth_image();
            cv::Mat rgbMat(color_image->get_height_pixels(), color_image->get_width_pixels(), CV_8UC3, (void *)(color_image->get_buffer()));
            cv::cvtColor(rgbMat, rgbMat, cv::COLOR_RGB2BGR);

            cv::Mat depthMat(depth_image->get_height_pixels(), depth_image->get_width_pixels(), CV_16UC1, (void *)(depth_image->get_buffer()));

            cv::imshow("rgbMat", rgbMat);
            cv::imshow("depthMat", depthMat);
            date.update();
            // cout << depthMat.rows << endl;
            // cout << depthMat.cols << endl;
            // cout << rgbMat.rows << endl;
            // cout << rgbMat.cols << endl;
            // char k =
            if (key == 's')
            {

                std::vector<int> compression_params;
                compression_params.push_back(cv::IMWRITE_PNG_COMPRESSION);
                compression_params.push_back(0);
                compression_params.push_back(cv::IMWRITE_PNG_STRATEGY);
                compression_params.push_back(cv::IMWRITE_PNG_STRATEGY_DEFAULT);
                cv::imwrite(std::string(date.fn) + std::string(date.rgbname), rgbMat, compression_params);
                cv::imwrite(std::string(date.fn) + std::string(date.depthname), depthMat, compression_params);
                printf("%d\n", cnt);
                fs << std::string(date.depthname) << " " << std::string(date.rgbname) << endl;

                cnt++;
            }
            if (key == 'q')
            {
                break;
                fs.close();
            }

            capture = nullptr;
        }
    }

    thread_quit = true;
    waitFrameThread.join();

    // Stop camera
    dev->stop_cameras();

    return 0;
}
CATCH_EXCEPTIONS()
